#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "LED.h"
#include "Timer.h"
#include "Key.h"
#include "Motor.h"
#include "Encoder.h"
#include "PID.h"
#include "Serial.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "cpp.h"

#define CENTRAL_ANGLE  0
#define CENTRAL_RANGE  20

uint8_t KeyNum;
uint8_t StartFlag;
//int16_t AX,AY,AZ,GX,GY,GZ;
float pitch,roll,yaw;

PID_t Angle={.Kp=5,.Ki=0,.Kd=26,.OutMax=100,.OutMin=-100};
PID_t Speed={.Kp=-0.6,.Ki=-0.004,.Kd=0,.Target=0,.OutMax=100,.OutMin=-100};

int main(void)
{
	OLED_Init();
	LED_Init();
	Key_Init();
//	Timer_Init();
	Motor_Init();
	Encoder_Init();
	Serial_Init();
	MPU_Init();
	mpu_dmp_init();
	while(1)
	{
		KeyNum=Key_GetNum();
		/*按键按下开始或结束平衡调控*/
		if(KeyNum==1)
		{
			StartFlag=!StartFlag;
		}
		mpu_dmp_get_data(&pitch,&roll,&yaw);  //获取MPU6050的姿态数据
		 
		OLED_Printf(0,0,OLED_8X16,"%f",pitch);
		OLED_ShowFloatNum(0,16,roll,3,1,OLED_8X16);
		OLED_ShowFloatNum(0,32,yaw,3,1,OLED_8X16);
		//角度环显示
//		OLED_Printf(0,0,OLED_6X8,"AngleRing:");
//		OLED_Printf(0,10,OLED_6X8,"Tar:%0.3f",Angle.Target);
//		OLED_Printf(0,20,OLED_6X8,"Act:%0.3f",Angle.Actual);
//		OLED_Printf(0,30,OLED_6X8,"Out:%0.3f",Angle.Out);
//		OLED_Printf(0,40,OLED_6X8,"Flag:%d",StartFlag);
//		
//		OLED_Printf(0,30,OLED_6X8,"Kp:%05.3f",Angle.Kp);
//		OLED_Printf(0,40,OLED_6X8,"Kd:%05.3f",Angle.Kd);
//		
//		//速度环显示
//		OLED_Printf(70,0,OLED_6X8,"SpeedRing:");
//		OLED_Printf(70,10,OLED_6X8,"Tar:%0.3f",Speed.Target);
//		OLED_Printf(70,20,OLED_6X8,"Act:%0.3f",Speed.Actual);
//		OLED_Printf(70,30,OLED_6X8,"Out:%0.3f",Speed.Out);
		
		OLED_Update();
	}	
}

/*由MPU6050的中断引脚每隔5ms产生一次中断*/
void EXTI1_IRQHandler()
{
	static uint8_t Count1,Count2;
	static float Angle0;
	if(EXTI_GetITStatus(EXTI_Line1)==SET)
	{	
		Count1++;
		
		if(Count1>=4)
		{	
			Count1=0;
			Key_Loop();  //按键扫描
		}
		
		Angle.Actual=roll;
		
		if(Angle0>CENTRAL_ANGLE+CENTRAL_RANGE||Angle0<CENTRAL_ANGLE-CENTRAL_RANGE)  //判断角度偏差是否超过范围
		{
			StartFlag=0;
		}
		else if(StartFlag==1)
		{
			Count2++;
			
			if(Count2>=5)
			{
				Count2=0;
				Speed.Actual=Encoder_GetLeftSpeed()+Encoder_GetRightSpeed();  //并级PID控制
				
				PID_Calculate(&Speed);  //速度环调控
				
			}
			PID_Calculate(&Angle);  //角度环调控
			
			float SetOut=Angle.Out+Speed.Out;
			
			Motor_SetLeftSpeed(SetOut);  //设置电机速度
			Motor_SetRightSpeed(SetOut);		
			}
		}
		
		if(StartFlag==0)
		{
			Motor_SetLeftSpeed(0);  //停止电机
			Motor_SetRightSpeed(0);
		}
		
		EXTI_ClearITPendingBit(EXTI_Line1);
}
